import os
import re
import numpy as np
import matplotlib.pyplot as plt


path = '/home/gy/.ros/log/latest' 
files = os.listdir(path)
mpc = 'movingPC.*'
myt = 'mytracking.*'

mpcFile = ''
mytFile = ''
re_mpc = None
re_myt = None
for ef in files:
    if re_mpc == None:
        re_mpc = re.match(mpc, ef)
        if not (re_mpc == None):
            mpcFile = os.path.join(path, re_mpc.group())
            
    if re_myt == None:
        re_myt = re.match(myt, ef)
        if not (re_myt == None):
            mytFile = os.path.join(path, re_myt.group())

print(mpcFile)
print(mytFile)

txy = []
with open(mpcFile, 'r') as f:    
    line = f.readline()
    # dataList = re.findall(r"[-+]?(?:\d*\.\d+|\d+)", line)
    # print(dataList)
    while(line):
        dataList = re.findall(r"[-+]?(?:\d*\.\d+|\d+)", line)
        data = [float(x) for x in dataList[1:4]]
        line = f.readline()
        txy.append(data)


ptxy = []
utxy = []
itxy = []
with open(mytFile, 'r') as f:    
    line = f.readline()
    # dataList = re.findall(r"[-+]?(?:\d*\.\d+|\d+)", line)
    # print(dataList)
    while(line):
        dataList = re.findall(r"[-+]?(?:\d*\.\d+|\d+)", line)
        data = [float(x) for x in dataList[1:6]]    # (time, pred_id, mode, x, y)
        
        if line.find('pred') >= 0:
            ptxy.append(data)
        elif line.find('inter') >= 0:
            itxy.append(data)
        elif line.find('upda') >= 0:
            utxy.append(data)
        else:
            pass
        line = f.readline()

txy = np.array(txy)
utxy = np.array(utxy)
ptxy = np.array(ptxy)
itxy = np.array(itxy)

# plt.subplot(1, 3, 1)
plt.plot(txy[:, 0], txy[:, 1], label='GT')

# c = '#c894c3'
# id = utxy[:, 1] == 0
# plt.plot(utxy[id, 0], utxy[id, 2], c=c, label='Update CV')
# id = utxy[:, 1] == 1
# plt.plot(utxy[id, 0], utxy[id, 2], c=c, ls=':', label='Update CTRV')
# id = utxy[:, 1] == 2
# plt.plot(utxy[id, 0], utxy[id, 2], c=c, ls='-.', label='Update RM')
# id = utxy[:, 1] == 3
# plt.plot(utxy[id, 0], utxy[id, 2], label='Meas')

n = 20
c = []
for i in range(1,1+n):
    c.append(list(np.random.rand(3,)));

for i in range(1,1+n):
    pid = ptxy[:, 1] == i
    if len(pid) == 0:
        break
    id = (pid & (ptxy[:, 2] == 0))
    plt.plot(ptxy[id, 0], ptxy[id, 3], c=c[i-1], label=str(i)+' Pred CV')
    id = (pid & (ptxy[:, 2] == 1))
    plt.plot(ptxy[id, 0], ptxy[id, 3], c=c[i-1], ls=':', label=str(i)+' Pred CTRV')
    id = (pid & (ptxy[:, 2] == 2))
    plt.plot(ptxy[id, 0], ptxy[id, 3], c=c[i-1], ls='-.', label=str(i)+' Pred RM')

# plt.legend()

# plt.subplot(1, 3, 2)
# plt.plot(txy[:, 0], txy[:, 2], label='GT')

# c = '#c894c3'
# id = utxy[:, 1] == 0
# plt.plot(utxy[id, 0], utxy[id, 3], c=c, label='Update CV')
# id = utxy[:, 1] == 1
# plt.plot(utxy[id, 0], utxy[id, 3], c=c, ls=':', label='Update CTRV')
# id = utxy[:, 1] == 2
# plt.plot(utxy[id, 0], utxy[id, 3], c=c, ls='-.', label='Update RM')
# id = utxy[:, 1] == 3
# plt.plot(utxy[id, 0], utxy[id, 3], label='Meas')

# for i in range(1,1+n):
    # pid = ptxy[:, 1] == i
    # if len(id) == 0:
        # break
    # id = ptxy[pid, 2] == 0
    # plt.plot(ptxy[id, 0], ptxy[id, 4], c=c[i], label=str(i)+'Pred CV')
    # id = ptxy[pid, 2] == 1
    # plt.plot(ptxy[id, 0], ptxy[id, 4], c=c[i], ls=':', label=str(i)+'Pred CTRV')
    # id = ptxy[pid, 2] == 2
    # plt.plot(ptxy[id, 0], ptxy[id, 4], c=c[i], ls='-.', label=str(i)+'Pred RM')

# # c = '#54ffb1'
# # id = ptxy[:, 1] == 0
# # plt.plot(ptxy[id, 0], ptxy[id, 3], c=c, label='Pred CV')
# # id = ptxy[:, 1] == 1
# # plt.plot(ptxy[id, 0], ptxy[id, 3], c=c, ls=':', label='Pred CTRV')
# # id = ptxy[:, 1] == 2
# # plt.plot(ptxy[id, 0], ptxy[id, 3], c=c, ls='-.', label='Pred RM')

# plt.legend()

# plt.subplot(1, 3, 3)
# plt.plot(txy[:, 1], txy[:, 2], label='GT')

# # c = '#c894c3'
# # id = utxy[:, 1] == 0
# # plt.plot(utxy[id, 2], utxy[id, 3], c=c, label='Update CV')
# # id = utxy[:, 1] == 1
# # plt.plot(utxy[id, 2], utxy[id, 3], c=c, ls=':', label='Update CTRV')
# # id = utxy[:, 1] == 2
# # plt.plot(utxy[id, 2], utxy[id, 3], c=c, ls='-.', label='Update RM')

# for i in range(1,1+n):
    # pid = ptxy[:, 1] == i
    # if len(id) == 0:
        # break
    # id = ptxy[pid, 2] == 0
    # plt.plot(ptxy[id, 3], ptxy[id, 4], c=c[i], label=str(i)+'Pred CV')
    # id = ptxy[pid, 2] == 1
    # plt.plot(ptxy[id, 3], ptxy[id, 4], c=c[i], ls=':', label=str(i)+'Pred CTRV')
    # id = ptxy[pid, 2] == 2
    # plt.plot(ptxy[id, 3], ptxy[id, 4], c=c[i], ls='-.', label=str(i)+'Pred RM')
# # id = utxy[:, 1] == 3
# # plt.plot(utxy[id, 2], utxy[id, 3], label='Meas')
# # plt.plot(itxy[:, 1], itxy[:, 2], label='Interation')

# # c = '#fcfe3c'
# # id = itxy[:, 1] == 0
# # plt.plot(itxy[id, 2], itxy[id, 3], c=c, label='Interation CV')
# # id = itxy[:, 1] == 1
# # plt.plot(itxy[id, 2], itxy[id, 3], c=c, ls=':', label='Interation CTRV')
# # id = itxy[:, 1] == 2
# # plt.plot(itxy[id, 2], itxy[id, 3], c=c, ls='-.', label='Interation RM')

# plt.legend()

plt.show()
        
        
